/*
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "esp_err.h"


#define GPIO_OUTPUT_IO_0       19
#define GPIO_OUTPUT_PIN_SEL  (1ULL<<GPIO_OUTPUT_IO_0)

#define SERVO_LS_TIMER          LEDC_TIMER_1
#define SERVO_LS_MODE           LEDC_LOW_SPEED_MODE

#define MOTOR_LS_TIMER          LEDC_TIMER_2
#define MOTOR_LS_MODE           LEDC_LOW_SPEED_MODE

#define SERVO_LS_CH0_GPIO       (3)
#define SERVO_LS_CH0_CHANNEL    LEDC_CHANNEL_0
#define MOTOR_LS_CH1_GPIO       (4)
#define MOTOR_LS_CH1_CHANNEL    LEDC_CHANNEL_1
#define MOTOR_LS_CH2_GPIO       (5)
#define MOTOR_LS_CH2_CHANNEL    LEDC_CHANNEL_2

#define PWM_CH_NUM              (3)
#define PWM_TEST_DUTY           (4000)
#define PWM_FADE_TIME           (100)

#define SERVO_CHANNEL           (0)
#define MOTOR_CHANNEL_LEFT      (1)
#define MOTOR_CHANNEL_RIGHT     (2)

#define SERVO_MIN_PULSEWIDTH 1000 //Minimum pulse width in microsecond
#define SERVO_MAX_PULSEWIDTH 2000 //Maximum pulse width in microsecond
#define SERVO_MAX_DEGREE 90 //Maximum angle in degree upto which servo can rotate

//定时器初始化
ledc_timer_config_t servo_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT, // resolution of PWM duty
        .freq_hz = 50,                      // frequency of PWM signal
        .speed_mode = SERVO_LS_MODE,           // timer mode
        .timer_num = SERVO_LS_TIMER,            // timer index
        .clk_cfg = LEDC_AUTO_CLK,              // Auto select the source clock
};

ledc_timer_config_t motor_timer = {
    .duty_resolution = LEDC_TIMER_13_BIT, // resolution of PWM duty
    .freq_hz = 5000,                        // frequency of PWM signal
    .speed_mode = MOTOR_LS_MODE,           // timer mode
    .timer_num = MOTOR_LS_TIMER,            // timer index
    .clk_cfg = LEDC_AUTO_CLK,              // Auto select the source clock
};

//定时器通道配置
ledc_channel_config_t pwm_channel[PWM_CH_NUM] = {
    {
        .channel    = SERVO_LS_CH0_CHANNEL,
        .duty       = 0,
        .gpio_num   = SERVO_LS_CH0_GPIO,
        .speed_mode = SERVO_LS_MODE,
        .hpoint     = 0,
        .timer_sel  = SERVO_LS_TIMER
    },
    {
        .channel    = MOTOR_LS_CH1_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR_LS_CH1_GPIO,
        .speed_mode = MOTOR_LS_MODE,
        .hpoint     = 0,
        .timer_sel  = MOTOR_LS_TIMER
    },
    {
        .channel    = MOTOR_LS_CH2_CHANNEL,
        .duty       = 0,
        .gpio_num   = MOTOR_LS_CH2_GPIO,
        .speed_mode = MOTOR_LS_MODE,
        .hpoint     = 0,
        .timer_sel  = MOTOR_LS_TIMER
    }
};

//gpio初始化
void hal_gpio_init(void)
{
    gpio_config_t config = {
        .pin_bit_mask = GPIO_OUTPUT_PIN_SEL, //引脚选择
        .mode = GPIO_MODE_OUTPUT, //输出模式                                
        .pull_up_en = GPIO_PULLUP_ENABLE, //使能上拉                          
        .pull_down_en = GPIO_PULLDOWN_DISABLE, //失能下拉                      
        .intr_type = GPIO_PIN_INTR_DISABLE, //失能中断                         
    };
    gpio_config(&config); //初始化
    gpio_set_level(GPIO_OUTPUT_IO_0, 1); //设置输出为高电平

}

//角度转换成US
static uint32_t servo_per_degree_init(uint32_t degree_of_rotation)
{
    uint32_t cal_pulsewidth = 0;
    cal_pulsewidth = (SERVO_MIN_PULSEWIDTH + (((SERVO_MAX_PULSEWIDTH - SERVO_MIN_PULSEWIDTH) * (degree_of_rotation)) / (SERVO_MAX_DEGREE)));
    return cal_pulsewidth;
}

//US转换成占空比
static uint32_t servo_per_duty_init(uint32_t time_us)
{
    uint32_t cal_duty = 0;
    cal_duty = 8192 * time_us / (20 * 1000);  //8192 = 2的LEDC_TIMER_13_BIT次方 - 1 ； time_us / (20 * 1000)指占空比时间占20ms整个周期的比例
    return cal_duty;
}

//设置pwm值
/**
 * @brief 
 * fade_time: us
 * 
 */
void set_pwm(uint16_t channel, uint32_t duty, int fade_time)
{
    ledc_set_fade_with_time(pwm_channel[channel].speed_mode, pwm_channel[channel].channel, duty, fade_time);
    ledc_fade_start(pwm_channel[channel].speed_mode,  pwm_channel[channel].channel, LEDC_FADE_NO_WAIT);
}

//gpio开关控制任务
void gpio_blink(void *arg)
{
   hal_gpio_init();

    while(1)
	{
        gpio_set_level(GPIO_OUTPUT_IO_0, 1);
		vTaskDelay(pdMS_TO_TICKS(50000)); //FreeRTOS延时函数，延时1秒
        gpio_set_level(GPIO_OUTPUT_IO_0, 0);
		vTaskDelay(pdMS_TO_TICKS(50000));
	}
}

//电机和舵机控制任务
void servo_motor_control(void *arg)
{
    int ch;

    ledc_timer_config(&servo_timer);
    ledc_timer_config(&motor_timer);

    // Set LED Controller with previously prepared configuration
    for (ch = 0; ch < PWM_CH_NUM; ch++) {
        ledc_channel_config(&pwm_channel[ch]);
    }

    // Initialize fade service.
    ledc_fade_func_install(0);

    while (1) {
        //舵机调试
        for (uint32_t count = 0; count < SERVO_MAX_DEGREE; count++) {
            printf("Angle of rotation: %d\n", count);
            uint32_t angle = servo_per_degree_init(count);
            printf("pulse width: %dus\n", angle);
            uint32_t duty = servo_per_duty_init(angle);
            printf("duty width: %d\n", duty);
            set_pwm(SERVO_CHANNEL, duty, PWM_FADE_TIME);
            vTaskDelay(10);     //Add delay, since it takes time for servo to rotate, generally 100ms/60degree rotation at 5V
        }

        //左右电机调试
        printf("motor fade up to duty = %d\n", PWM_TEST_DUTY);
        set_pwm(MOTOR_CHANNEL_LEFT, PWM_TEST_DUTY, PWM_FADE_TIME);
        set_pwm(MOTOR_CHANNEL_RIGHT, PWM_TEST_DUTY, PWM_FADE_TIME);
        vTaskDelay(PWM_FADE_TIME / portTICK_PERIOD_MS);

        
        set_pwm(MOTOR_CHANNEL_LEFT, 0, PWM_FADE_TIME);
        set_pwm(MOTOR_CHANNEL_RIGHT, 0, PWM_FADE_TIME);
        vTaskDelay(PWM_FADE_TIME / portTICK_PERIOD_MS);
    }

}

void app_main(void)
{

    printf("START!!!\n");

    xTaskCreate(gpio_blink, "gpio_blink", 4096, NULL, 5, NULL);

    xTaskCreate(servo_motor_control, "servo_control", 4096, NULL, 5, NULL);

}
